#ifndef _KINEMATICS_H_
#define _KINEMATICS_H_

#include <math.h>

#define PI 3.1415926

typedef struct{
    float x;
    float y;
    float z;
} Point;

typedef struct{
    float joint1angle;
    float joint2angle;
} JointAngle;

typedef struct{     //standard D_H
    float theta;    //joint angle
    float b;        //joint offset
    float a;        //link length
    float alpha;    //Link twist
} DH;

typedef struct MATRIX{
    float m[4][4];
} Matrix;

Matrix matrixMultiply(Matrix A, Matrix B);
Matrix DH2Matrix(DH params);
Point getPointFromMatrix(Matrix Mat);
Point forwardKinematics(JointAngle Angle);
JointAngle inversekinematics(Point endpoint);

#endif